#!/usr/bin/python
# -*- mode:python; coding:utf-8 -*-

# NOTE: see https://code.ros.org/trac/opencv/ticket/373 if get
#  "cv.error: Non-positive width or height"

import cv
import sys
import math

if len(sys.argv) != 2:
    print "Usage: %s <image>" % sys.argv[0]
    sys.exit(1)

def getSquare(p1, p3):
    x1, y1 = p1
    x3, y3 = p3

    r = math.sqrt((x3-x1)**2 + (y3-y1)**2) / 2
    xc, yc = x1 + (x3-x1)/2, y1 + (y3-y1)/2
    m = (x3-x1) / float(y1-y3)

    cosm = r * r * (y1-y3) / (r * math.sqrt((y1-y3)**2 + (x3-x1)**2))

    x = xc + cosm
    y = m*x - m*xc + yc
    p2 = int(x), int(y)

    x = xc - cosm
    y = m*x - m*xc + yc
    p4 = int(x), int(y)

    return p1, p2, p3, p4


im = cv.LoadImage(sys.argv[1], cv.CV_LOAD_IMAGE_GRAYSCALE)
keys = cv.ExtractSURF(im, None, cv.CreateMemStorage(), (0, 300, 3, 2))[0]
if len(keys) == 2:
    p1 = tuple(map(int, keys[0][0]))
    p3 = tuple(map(int, keys[1][0]))
    p1, p2, p3, p4 = getSquare(p1, p3)
    cv.PolyLine(im, [[p1, p2, p3, p4]], 1, 125, 2)

cv.NamedWindow("Test 07")
cv.StartWindowThread()
cv.ShowImage("Test 07", im)
cv.WaitKey()






